package fr.imag.m2pgi.m2m.geoloc.scip;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.Iterator;
import java.util.Map;

//import gnu.io.CommPort;
//import gnu.io.CommPortIdentifier;
//import gnu.io.NoSuchPortException;
//import gnu.io.PortInUseException;
//import gnu.io.SerialPort;

public class SCIP20 {
  /** First detection step **/
  private static final int FIRST_DETECTION_STEP = 44;
  /** Last detection step **/
  private static final int LAST_DETECTION_STEP = 725;
  /** First detection step **/
  private static final int CLUSTER = 1;

  /** Lidar's resolution **/
  private static final int RESOLUTION = 1024;

  /** Version command **/
  private static final byte[] VV = "VV".getBytes();
  /** Specification command **/
  private static final byte[] PP = "PP".getBytes();
  /** Enabling command **/
  private static final byte[] BM = "BM".getBytes();
  /** Disabling command **/
  private static final byte[] QT = "QT".getBytes();
  /** 3 characters last measure command **/
  private static final byte[] GD = "GD".getBytes();
  /** 2 characters last measure command **/
  private static final byte[] GS = "GS".getBytes();
  /** 2 characters last measure command **/
  private static final byte[] II = "II".getBytes();

  /** LF character **/
  private static final byte LF = 0x0A;
  /** 00P, sum for version (VV) **/
  private static final byte[] OOP = "00P".getBytes();

  /** Vendor Information. */
  public static final String KEY_VENDOR = "Vender Information"; // sic
  /** Product Information. */
  public static final String KEY_PRODUCT = "Product Information";
  /** Firmware Version. */
  public static final String KEY_FIRMWARE = "Firmware Version";
  /** Protocol Version. */
  public static final String KEY_PROTOCOL = "Protocol Version";
  /** Sensor Serial Number. */
  public static final String KEY_SERIAL = "Sensor Serial Number";
  /** Undocumented Status Field. */
  public static final String KEY_STATUS = "Undocumented Status Field";

  /** Sensor model Information. */
  public static final String SENSOR_MODEL = "Sensor Model"; // sic
  /** Minimum measurement Information. */
  public static final String MIN_MEASUREMENT = "Minimum Measurement";
  /** Max measurement Information. */
  public static final String MAX_MEASUREMENT = "Maximum Measurement";
  /** Total step in 360deg */
  public static final String TOTAL_STEPS = "Total steps in 360 degree";
  /** First step of measurement range. */
  public static final String FIRST_STEP_MEASURE = "First step of measurement range";
  /** Last step of measurement range. */
  public static final String LAST_STEP_MEASURE = "Last step of measurement range";
  /** Front step number. */
  public static final String FRONT_STEP = "Front step number";
  /** Motor speed (lol). */
  public static final String MOTOR_SPEED = "Motor speed";
  
  private InputStream is;
  private OutputStream os;

  public SCIP20(int port) throws IOException {
    Socket socket = new Socket("localhost", port);
    is = socket.getInputStream();
    while(is.available()!=0)
      is.read();
    os = socket.getOutputStream();
  }

//  public SCIP20(String portName) {
//    CommPortIdentifier portIdentifier;
//    try {
//      portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
//    } catch (NoSuchPortException e) {
//      throw new SCIPException(e);
//    }
//    if (portIdentifier.isCurrentlyOwned()) {
//      throw new SCIPException("Port already in use");
//    }
//    try {
//      CommPort port = portIdentifier.open(this.getClass().getName(), 2000);
//      if (port instanceof SerialPort) {
//        SerialPort sPort = (SerialPort) port;
//        sPort.setSerialPortParams(57600, SerialPort.DATABITS_8,
//            SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
//        is = sPort.getInputStream();
//        os = sPort.getOutputStream();
//      } else {
//        throw new SCIPException("No a serial port");
//      }
//    } catch (PortInUseException e) {
//      throw new SCIPException("Port already in use");
//    } catch (Exception e) {
//      throw new SCIPException(e);
//    }
//  }

//  static void listPorts() {
//    java.util.Enumeration<CommPortIdentifier> portEnum = CommPortIdentifier
//        .getPortIdentifiers();
//    while (portEnum.hasMoreElements()) {
//      CommPortIdentifier portIdentifier = portEnum.nextElement();
//      System.out.println(portIdentifier.getName() + " - "
//          + getPortTypeName(portIdentifier.getPortType()));
//    }
//    System.out.println("Listed all ports.");
//  }
//
//  static String getPortTypeName(int portType) {
//    switch (portType) {
//    case CommPortIdentifier.PORT_I2C:
//      return "I2C";
//    case CommPortIdentifier.PORT_PARALLEL:
//      return "Parallel";
//    case CommPortIdentifier.PORT_RAW:
//      return "Raw";
//    case CommPortIdentifier.PORT_RS485:
//      return "RS485";
//    case CommPortIdentifier.PORT_SERIAL:
//      return "Serial";
//    default:
//      return "unknown type";
//    }
//  }

  public synchronized Map<String, String> getVersion() throws IOException {
    // Write command
    os.write(VV);
    os.write(LF);
    os.flush();
    // Now read response
    // Read send command
    read(VV);
    readLF();
    read(OOP);
    readLF();
    // Now read
    HashMap<String, String> map = new HashMap<String, String>();
    map.put(KEY_VENDOR, readString());
    map.put(KEY_PRODUCT, readString());
    map.put(KEY_FIRMWARE, readString());
    map.put(KEY_PROTOCOL, readString());
    map.put(KEY_SERIAL, readString());
    readLF(); // end of response
    return map;
  }

  public synchronized int getDistance(double angle) throws IOException {
    return getDistance(getTwoCharLastMeasure(), angle);
  }
  public synchronized int getDistance(ArrayList<Integer> data, double angle)
      throws IOException {
    if (angle < -120 || angle > 120)
      throw new IllegalArgumentException("Angle out of range: " + angle);
    double convertAngle = Math.abs(angle - 120.);
    int data_point = (int) (convertAngle * ((double)RESOLUTION / 360d));
//    int data_point = (int) (convertAngle * (RESOLUTION / 360));
    if(data_point==data.size())
      data_point--;
    return data.get(data_point);
  }

  public synchronized ArrayList<Integer> getTwoCharLastMeasure() throws IOException {
    return getTwoCharLastMeasure(FIRST_DETECTION_STEP, LAST_DETECTION_STEP, CLUSTER);
  }
  public synchronized ArrayList<Integer> getTwoCharLastMeasure(int firstStep, int lastStep, int cluster)
      throws IOException {
    byte[] req = new byte[13];
    ByteBuffer bb = ByteBuffer.wrap(req);
    //Command
    bb.put(GS);
    //First step
    bb.put(format4(firstStep));
    //Last step
    bb.put(format4(lastStep));
    /* 
     * Cluster count: nb of steps merged into one cluster 
     * (value is the lowest of all steps' values)
     */
    bb.put(format2(cluster));
    //End of command
    bb.put(LF);
    os.write(req);
    os.flush();
    
    //Reception
    //Command copy
    read(req);
    
    //Status
    try {
      readStatus();
    } catch(SCIPException e) {
      System.err.println("Caught exception. Request: "+new String(req));
      throw e;
    }
    
    //Timestamp
    byte[] time = { (byte)is.read(), (byte)is.read(),
                    (byte)is.read(), (byte)is.read()};
    int timestamp = getFourCharValue(time[0], time[1],
                                      time[2],time[3]);
    int sum = (byte)is.read();
    if(sum != verifyCKS(time))
      System.out.println("Problem with CKS: "+sum+" (verifyCKS: "+verifyCKS(time)+")");
    
    readLF();
    
    /* Data
     * [Data Block 1   (64bytes)][Sum][LF]
     * ...
     * [Data Block N-1 (64bytes)][Sum][LF]
     * [Data Block N   (n bytes)][Sum][LF][LF]
     */
    //TODO: initial capacity
    ArrayList<Integer> res = new ArrayList<Integer>();
    int count = 0;
    byte b1, b2;
    byte[] b = new byte[64];
    ByteBuffer bb2 = ByteBuffer.wrap(b);
    int zeros = 0;
    while((b1 = (byte) is.read()) != LF) {
      b2 = (byte) is.read();
      if(b2==LF)
        break;
      if(b2=='0' && b1=='0')
        zeros++;
      res.add(getTwoCharValue(b1, b2));
      bb2.put(b1).put(b2);
      count +=2;
      if(count==64) {
        sum = is.read();
        if(sum != verifyCKS(bb2.array()))
          System.out.println("Problem with CKS: "+sum+" (verifyCKS: "+verifyCKS(bb2.array())+")");
        count = 0;
        bb2.clear();
        readLF();
      }
    }
//    System.err.println("Read "+zeros+" zero values");
    if(b1!=LF)
      readLF();
    return res;  
  }

  public synchronized void enableLaser() throws IOException {
    // Write command
    os.write(BM);
    os.write(LF);
    os.flush();

    // Read response
    read(BM);
    readLF();
    readStatus();
    readLF(); // end of response
  }

  public synchronized void disableLaser() throws IOException {
    // Write command
    os.write(QT);
    os.write(LF);
    os.flush();

    // Read response
    read(QT);
    readLF();
    read(OOP);
    readLF();
    readLF(); // end of response
  }

  public synchronized Map<String, String> getSpecification() throws IOException {
    // Write command
    os.write(PP);
    os.write(LF);
    os.flush();
    // Now read response
    // Read send command
    read(PP);
    readLF();
    read(OOP);
    readLF();
    // Now read
    HashMap<String, String> map = new HashMap<String, String>();
    map.put(SENSOR_MODEL, readString());
    map.put(MIN_MEASUREMENT, readString());
    map.put(MAX_MEASUREMENT, readString());
    map.put(TOTAL_STEPS, readString());
    map.put(FIRST_STEP_MEASURE, readString());
    map.put(LAST_STEP_MEASURE, readString());
    map.put(FRONT_STEP, readString());
    map.put(MOTOR_SPEED, readString());
    readLF(); // end of response
    return map;
  }
  
  public synchronized boolean getState() throws IOException {
    System.out.println("Test");
    // Write command
    os.write(II);
    os.write(LF);
    os.flush();
    System.out.println("Commande envoyée");
    // Now read response
    // Read send command
    read(II);
    readLF();
    System.out.println("Commande lue");
    read(OOP);
    readLF();
    System.out.println("Status lu");
    readString();
    String[] illumin = readString().split(":");
//    System.out.println(illumin[0]+" ; "+illumin[1]);
    String state = illumin[1].substring(0, illumin[1].length() - 2);
//    System.out.println("state: "+state);
    for(int i=0; i < 5; i++)
    {
      readString();
    }
    readLF();
    System.out.println("Data lues");
    System.out.println("Fin test");
    return state.equals("ON");
  }
  
  static byte verifyCKS(byte[] data)
  {
    int sum=0;
    for(int i=0; i < data.length; i++)
    {
      sum += data[i];
    }
    sum &= 0x3F;
    sum += 0x30;
    return (byte) sum;
  }
  
  private void readLF() throws IOException {
    byte read = (byte) is.read();
    if (read != LF)
      throw new SCIPException("Read " + read + " instead of " + LF);
  }
  
  private byte[] format4(int i) {
    byte[] res = new byte[4];
    res[0] = (byte) ((i/1000)%10 + 0x30);
    res[1] = (byte) ((i/100) %10 + 0x30);
    res[2] = (byte) ((i/10)  %10 + 0x30);
    res[3] = (byte) ( i      %10 + 0x30);
    return res;
  }
  private byte[] format2(int i) {
    byte[] res = new byte[2];
    res[0] = (byte) ((i/10)%10 + 0x30);
    res[1] = (byte) ( i    %10 + 0x30);
    return res;
  }

  public int readStatus() throws IOException {
    int status  = (is.read()-'0') * 10; 
        status += (is.read()-'0');
//    System.out.println("status: " + status);
    if (status == 0) {
      int sum = is.read();
      readLF();
      return 0;
    } else {
      is.read();
      readLF();
      readLF();
      throw new SCIPException("Incorrect status: "+status);
    }
  }

  private void read(byte[] expected) throws IOException {
    byte[] read = new byte[expected.length];
    is.read(read);
    if (!Arrays.equals(expected, read)) {
      throw new SCIPException("Read " + new String(read) + " instead of "
          + new String(expected));
    }
  }
  
  private String readString() throws IOException {
    StringBuilder sb = new StringBuilder();
    int b;
    while ((b = is.read()) != -1 && b != LF) {
      sb.append((char) b);
    }
    return sb.toString();
  }

  private int getTwoCharValue(byte c1, byte c2) {
    int res = ((c1 - 0x30) & 0x3F) << 6;
    res    |=  (c2 - 0x30) & 0x3F;
    return res;
  }

  private int getThreeCharValue(byte c1, byte c2, byte c3) {
    int res = ((c1 - 0x30) & 0x3F) << 12;
    res    |= ((c2 - 0x30) & 0x3F) << 6;
    res    |= ((c3 - 0x30) & 0x3F);
    return res;
  }
  private int getFourCharValue(byte c1, byte c2, byte c3, byte c4) {
    int res = ((c1 - 0x30) & 0x3F) << 18;
    res    |= ((c2 - 0x30) & 0x3F) << 12;
    res    |= ((c3 - 0x30) & 0x3F) << 6;
    res    |= ((c4 - 0x30) & 0x3F);
   return res;
  }

  public static void main(String[] args) {
    // listPorts();

    // SCIP20 laser = new SCIP20("/dev/ttyACM0");
    try {
      SCIP20 laser = new SCIP20(1701);
      Map<String, String> m = laser.getVersion();
      System.out.println(m.get(KEY_VENDOR));
      System.out.println(m.get(KEY_PRODUCT));
      System.out.println(m.get(KEY_FIRMWARE));
      System.out.println(m.get(KEY_PROTOCOL));
      System.out.println(m.get(KEY_SERIAL));
      
      Map<String, String> specif = laser.getSpecification();
      System.out.println(specif.get(SENSOR_MODEL));
      System.out.println(specif.get(MIN_MEASUREMENT));
      System.out.println(specif.get(MAX_MEASUREMENT));
      System.out.println(specif.get(TOTAL_STEPS));
      System.out.println(specif.get(FIRST_STEP_MEASURE));
      System.out.println(specif.get(LAST_STEP_MEASURE));
      System.out.println(specif.get(FRONT_STEP));
      System.out.println(specif.get(MOTOR_SPEED));

      System.out.println("Enabling laser...");
      laser.enableLaser();
      System.out.println("Getting 12bits measures...");
      ArrayList<Integer> points = laser.getTwoCharLastMeasure();
      System.out.println("Measured "+points.size()+" points.");
      System.out.println("Disable laser...");
      laser.disableLaser();
      System.out.println("Values:");
      int i;
      for(i=0; i<points.size(); i++) {
        System.out.println("["+i+"] : "+points.get(i));
      }
      // for (String key : m.keySet()) {
      // System.out.println(key + " : " + m.get(key));
      // }
    } catch (IOException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
  }

}
